﻿
// 2024/2/26: 首个版本
// 2024/7/3: 基于新样本框架
// 2025/8/4: 不使用cat宏
// 2025/8/4: 改为继承SessionSample。改回单头文件

#ifndef WALKER_CONTROL_V1_H
#define WALKER_CONTROL_V1_H

#include "spadas.h"

namespace walker_control_v1
{
	using namespace spadas;

	struct WalkerControlData
	{
		Bool requestControl; // Weather to request control / 是否请求控制
		Double heading; // [deg] Heading of moving, 0 as north, CCW is positive / 行进朝向，0为正北，逆时针为正
		Double speed; // [KPH] Moving speed, 0 is static / 移动速度，0为静止

		WalkerControlData()
		{
			requestControl = FALSE;
			heading = 0;
			speed = 0;
		}
	};

	class WalkerControl : public SessionSample
	{
	private:
		class WalkerControlVars : public SessionSampleVars
		{
		public:
			SPADAS_VARS(WalkerControl, SessionSampleVars)
			WalkerControlData data;

			SessionGeneralSample toGeneralSample()
			{
				SessionGeneralSample sample;
				sample.timestamp() = timestamp;

				Array<GeneralElement> values(3);
				sample.values() = values;

				values[0] = data.requestControl ? 1 : 0;
				values[1] = data.heading;
				values[2] = data.speed;

				sample.significantCount() = values.size();
				return sample;
			}

			Bool fromSample(String protocol, SessionSample sample) override
			{
				SessionGeneralSample generalSample;
				if (!sample.is<SessionGeneralSample>(generalSample)) return FALSE;

				timestamp = generalSample.timestamp();
				auto values = generalSample.values();

				if (protocol == "walker-control-v1")
				{
					if (values.size() != 3) return FALSE;

					data.requestControl = values[0].valid && values[0].value == 1;
					data.heading = values[1].value;
					data.speed = values[2].value;

					return TRUE;
				}

				return FALSE;
			}
		};

	public:
		SPADAS_CLASS("walker_control_v1.WalkerControl", WalkerControlVars)

		WalkerControl() : SessionSample(new WalkerControlVars)
		{}
		WalkerControlData *operator ->()
		{
			return &var()->data;
		}

		static String getProtocolName(UInt channel)
		{
			return (String)"walker-control-v1@" + channel;
		}
		SessionGeneralSample toGeneralSample()
		{
			return var()->toGeneralSample();
		}

		static Array<String> getProtocolNames(UInt channel)
		{
			return Array<String>::create(1, "walker-control-v1").convert<String>([channel](auto& s){ return s + "@" + channel; });
		}
		Bool fromGeneralSample(String protocol, SessionGeneralSample generalSample)
		{
			return fromSample(protocol, generalSample);
		}
	};
}

#endif
